#pragma once

#include "VisioCommon.h"
#include "Camera.h"
#include "PointCloud.h"
#include "Util.h"
#include "..\MathAux\sba-1.5\sba.h"

typedef std::vector<int> InlinersVector;

struct SInliners
{
	InlinersVector WeakInliners;
	InlinersVector Inliners;
};

class CSfmPool
{
public:
	UINT AddCameras(CamerasVector& cameras, CPointCloud& PointCloud);
	CCamera* FindCameraFromProjection(CImageData* pImageData, CCameraMesh* pMesh, const VisiblePointsVector& SharedVisablePoints,const CPointCloud& PointCloud, SInliners& inliners);
	SCameraParams FindCameraParamsFromProjection(double* points, double* projections, UINT NumPoints, int& NumInliners);
	SInliners CalculateInlinersOfCamera(const CCamera* pCam, double* points, double* projections, int NumInliners);
	void RefineCameraParams(CCamera* pCam, double* points,double* projections, SInliners& inliners);
	Mat FixIntrinsincsMatrix(Mat mP, Mat mKInit);
	InlinersVector RefineCameraParamsInternal( CCamera* pCam, double* points,double* projections, InlinersVector& inliners, UINT NumInliners);
	void CheckAddedIndex(CamerasVector& cams);
	void RunSFM(CamerasVector& cameras, CPointCloud& PointCloud, char* vmask, double* projections, cv::Point3d* points, int NumPoints, int* remap);
	UINT RemoveOutliers( CamerasVector& cameras, CPointCloud& PointCloud, char* vmask, double* projections, cv::Point3d* points, int* remap );
	void CameraRefine(CCamera* pCam, double* points, double* projections, UINT NumPoints, bool bEstimateIntrin);
	void ReadBundlerParams(double* _params, double* _projections, char* _vmask, int NumParams);
private:

};